#include <cassert>
#include <boost/algorithm/string.hpp>
#include <map>

#include "../../../XMLTools.h"
#include "../../../Exceptions.h"

#include "MoCapMarkerSensor.h"

using namespace MMM;

MoCapMarkerSensorPtr MoCapMarkerSensor::loadSensorXML(RapidXMLReaderNodePtr node) {
    MoCapMarkerSensorPtr sensor(new MoCapMarkerSensor());
    sensor->loadSensor(node);
    return sensor;
}

MoCapMarkerSensor::MoCapMarkerSensor(const std::string &description) : InterpolatableSensor(description)
{
}

bool MoCapMarkerSensor::checkModel(ModelPtr model) {
    return true;
}

boost::shared_ptr<BasicSensor<MoCapMarkerSensorMeasurement> > MoCapMarkerSensor::cloneConfiguration() {
    MoCapMarkerSensorPtr m(new MoCapMarkerSensor(description));
    return m;
}

void MoCapMarkerSensor::loadConfigurationXML(RapidXMLReaderNodePtr node) {
    assert(node);
    assert(node->name() == "Configuration");
}

void MoCapMarkerSensor::loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type)
{
    assert(node);
    assert(node->name() == XML::NODE_MEASUREMENT);

    std::map<std::string, Eigen::Vector3f> markers;
    std::vector<Eigen::Vector3f> unlabeledMarkers;
    for (auto markerNode : node->nodes("MarkerPosition")) {
        std::string name = markerNode->attribute_value_or_default("name", "");
        Eigen::Vector3f markerPosition = XML::readFloatArray(markerNode->value(), 3);
        if (name.empty()) unlabeledMarkers.push_back(markerPosition);
        else if (markers.find(name) == markers.end()) markers[name] = markerPosition;
        else throw Exception::XMLFormatException("Duplicate of marker '" + name + "' at timestep '" + XML::toString(timestep) + "' in MoCapMarker sensor data");
    }
    MoCapMarkerSensorMeasurementPtr m(new MoCapMarkerSensorMeasurement(timestep, markers, unlabeledMarkers, type));
    addSensorMeasurement(m);
}

void MoCapMarkerSensor::appendConfigurationXML(RapidXMLWriterNodePtr node)
{
    assert(node);

    node->append_node("Configuration");
}

bool MoCapMarkerSensor::hasSensorConfigurationContent() const
{
    return true; // TODO
}

bool MoCapMarkerSensor::equalsConfiguration(SensorPtr other) {
    MoCapMarkerSensorPtr ptr = boost::dynamic_pointer_cast<MoCapMarkerSensor>(other);
    if (ptr) {
        return true;
    }
    else return false;
}

std::map<float, std::map<std::string, Eigen::Vector3f> > MoCapMarkerSensor::getLabeledMarkerData() {
    std::map<float, std::map<std::string, Eigen::Vector3f>> m;
    for (const auto &measurement : measurements) m.insert(std::pair<float, std::map<std::string, Eigen::Vector3f>>(measurement.first, measurement.second->getLabeledMarker()));
    return m;
}

std::string MoCapMarkerSensor::getType() {
    return TYPE;
}

std::string MoCapMarkerSensor::getVersion() {
    return VERSION;
}

int MoCapMarkerSensor::getPriority() {
    return 20;
}
